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Abstract. This paper develops numerical methods for optimal control 
of mechanical systems in the Lagrangian setting. It extends the theory 
^N of discrete mechanics to enable the solutions of optimal control prob- 

lems through the discretization of variational principles. The key point 
is to solve the optimal control problem as a variational integrator of a 
specially constructed higher-dimensional system. The developed frame- 
work applies to systems on tangent bundles, Lie groups, underactuated 
and nonholonomic systems with symmetries, and can approximate ei- 
ther smooth or discontinuous control inputs. The resulting methods in- 
herit the preservation properties of variational integrators and result in 
numerically robust and easily implementable algorithms. Several the- 
I I oretical and a practical examples, e.g. the control of an underwater 

\^ vehicle, will illustrate the application of the proposed approach. 
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1. Introduction 



The goal of this paper is to develop, from a geometric point of view, nu- 
merical methods for optimal control of Lagrangian mechanical systems. Our 
,__! approach employs the theory of discrete mechanics and variational integra- 

J> tors [31j to derive both an integrator for the dynamics and an optimal control 

^^ algorithm in a unified manner. This is accomplished through the discretiza- 

jy^ tion of the Lagrange-d'Alembert variational principle on manifolds. An in- 

(^ tegrator for the mechanics is derived using a standard Lagrangian function 

*y^ and virtual work done by control forces, while control optimality conditions 

(^ are derived using a special Lagrangian defined on a higher-dimensional space 

^N which encodes the dynamics and a desired cost function. The resulting in- 

. . tegration and optimization schemes are symplectic, respect the state space 

_^ structure, and momentum preserving. These qualities are associated with 

K> numerical stability which motivate the development of practical algorithms 

that can be applied to robotic or aerospace vehicles. 

The proposed framework is general and applies to unconstrained sys- 
tems, as well systems with symmetries, underactuation, and nonholonomic 
constraints. In particular, our construction is appropriate for controlled 
Lagrangian systems that evolve on a general tangent bundle TQ with as- 
sociated discrete state space Q x Q, where Q is a differentiable manifold 
([311135]). In addition we focus on underactuated systems evolving on a Lie 
group G ([H El ISl EI]) that are applicable for systems consisting of rigid 
bodies. Finally, the theory extends to the more general principle bundle 
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setting with discrete analog Q x Q x G (or more generally {Q x Q)/G) as- 
suming that the action of a Lie group G of symmetry leaves the control 
system invariant ([9tlTH[T9]). 

The main idea is the following: we take an approximation of the Lagrange- 
d'Alembert principle for forced Lagrangian systems, which models control 
inputs and external forces such as gravity or drag. In principle, we admit 
the possibility of piecewise continuous control forces, as happens in real ap- 
plications. We observe that the discrete equations of motion for this type of 
systems are interpreted as the discrete Euler-Lagrange equations of a new 
Lagrangian defined in an augmented discrete phase space. Next, we apply 
discrete variational calculus techniques to derive the discrete optimality con- 
ditions. After this, we recover two sequences of discrete controls modeling 
a piecewise control trajectory. 

Additionally we show how to derive the equations for different reduced 
systems. We specifically develop numerical methods for systems on Lie 
groups that lead to practical algorithm implementation. One such example 
system-an underactuated underwater vehicle-is used to illustrate the devel- 
oped methodology. The resulting algorithm is simple to implement and has 
the ability to quickly converge to a solution which is close to the optimal 
solution and to the true system dynamics. We also extend our techniques to 
more general reduced systems like optimal control problems in trivial prin- 
cipal bundles and we show how to introduce nonholonomic constraints in 
our framework. 

Moreover, since we are reducing the optimality conditions to discrete 
Euler-Lagrange equations, the geometric preservation properties like sym- 
plectic-momentum preservation in the standard case or Poisson bracket and 
momentum preservation for reduced systems are automatically guaranteed 
using the results in [25l |3l] . 

The paper is structured as follows: ^introduces variational integrators. 
^ formulates optimal control problems for Lagrangian systems defined on 
tangent bundles, in the continuous and discrete setting, and for both fully 
and underactuated systems. A simple control problem for a mechanical La- 
grangian on W^ illustrates these developments. In Q discrete mechanics on 
Lie groups is introduced. Specifically, discrete Euler-Poincare equations and 
their Hamiltonian version, the discrete Lie-Poisson equations, are obtained. 
Sections ^ and ^ develop the discretization procedure and the numerical 
aspects of the proposed approach. The developed algorithm is illustrated 
with an application to an unmanned underwater vehicle evolving on S'£'(3). 
Finally, ^ deals with reduced systems on a trivial principal bundle and with 
nonholonomic mechanics. 



2. Discrete Mechanics and Variational Integrators 

Let Q be a n-dimensional differentiable manifold with local coordinates 
((?*), 1 < i < n. Denote by TQ its tangent bundle with induced coordi- 
nates (g*, g*). Given a Lagrangian function L: TQ — )• R the Euler-Lagrange 
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equations are 

d (dL\ dL 

These equations are a system of implicit second order differential equations. 
In the sequel, we will assume that the Lagrangian is regular, that is, the 
matrix ( ^^^ .^ J is non-singular. It is well known that the origin of these 

equations is variational (see [U |3D] ) . 

Variational integrators retain this variational character and also some of 
main geometric properties of the continuous system, such as symplecticity 
and momentum conservation (see [12j and references therein). 

In the following we will summarize the main features of this type of nu- 
merical integrators |31j . A discrete Lagrangian is a map L,^ : Q x Q — ?■ H, 
which may be considered as an approximation of the integral action defined 
by a continuous Lagrangian L: TQ — )• R: L(i{qo,qi) ~ /g L{q{t),q{t)) dt 
where q{t) is a solution of the Euler-Lagrange equations for L, where g(0) = 
qo and q{h) = qi and /i > is enough small. 

Remark 2.1. The Cartesian product Q xQ is equipped with an interesting 
differential structure, the Lie groupoid structure which allows us to extend 
the construction of variational calculus to another interesting situations (Lie 
groupoids). See [25j for more details. 

Define the action sum Sd : Q ~^ ^^ corresponding to the Lagrangian 

Ld by Sd = ^k=i^d{qk-i,qk), where qk £ Q (oi < k < N, and N is the 
number of steps. The discrete variational principle states that the solutions 
of the discrete system determined by Ld must extremize the action sum 
given fixed endpoints go and q^- By extremizing Sd over g^, l<fc<A^ — 1, 
we obtain the system of difference equations 

DiLdiqk, qk+i) + D2Ld{qk-i,qk) = 0, (2) 



or, in coordinates. 



-jiQk, qk+i) + -7r-[qk-i,qk) = 0, 



where l<i<n, l<fc<A^ — 1 and x, y denote the n-first and n-second 
variables of the function L respectively. 

These equations are usually called the discrete Euler— Lagrange equa- 
tions. Under some regularity hypotheses (the matrix {Di2Ld{qk, qk+i)) is 
regular), it is possible to define a (local) discrete flow T^^ : Q x Q ^ Q x Q, 
by T L^{qk~i,qk) = {qk,qk+i) from (pi). Define the discrete Legendre trans- 
formations associated to Ld as 

F-Ld-.QxQ -^ T*Q 

{Q0,qi) I — > {qo,-DiLd{qo,qi)), 

F+Ld-.QxQ -^ T*Q 

{qo,qi) I — > {qi,D2Ld{qo,qi)) , 

and the discrete Poincare-Cartan 2-form Ud = (F~^Ld)*u>Q = (F^Ld)*u>Q, 
where cjq is the canonical symplectic form on T*Q. The discrete algorithm 
determined by T/,^ preserves the symplectic form Ud, i.e., T^ cod = ^d- 
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Moreover, if the discrete Lagrangian is invariant under the diagonal action 
of a Lie group G, then the discrete momentum map J^ : Q x Q — )• 0* defined 

by 

{Jd{qk,qk+i),0 = {D2Ld{qk,qk+i),^Q{qk+i)) 

is preserved by the discrete flow. Therefore, these integrators are symplectic- 
momentum preserving. Here, S,q denotes the fundamental vector field de- 
termined by ^ G 0, where g is the Lie algebra of G. (See |31j for more 
details.) 

3. Discrete optimal control on tangent bundles 

Consider a mechanical system which configuration space is a n-dimensional 
differentiable manifold Q and which dynamics is determined by a Lagrangian 
L : TQ — >■ R. The control forces are modeled as a mapping / : TQ x [/ — )• 
T*Q, where f{vq,u) € T*Q, Vq G TqQ and u £ U, being U the control 
space. Observe that this last definition also covers configuration and veloc- 
ity dependent forces such as dissipation or friction (see [35]). For greater 
generality we consider control variables that are only piecewise continuous 
to account for impulsive controls. 

The motion of the mechanical system is described by applying the princi- 
ple of Lagrange-D'Alembert, which requires that the solutions q{t) G Q 
must satisfy 

L{q{t),q{t))dt+ [ f{q{t),q{t),u{t))5q{t)dt = 0, (3) 

Jo 

where {q , q) are the local coordinates of TQ and where we consider arbitrary 
variations 6q G Tq^f-^Q with 5q{0) = and 6q{T) = (since we are prescribing 
fixed initial and final conditions {qiO),q{0)) and (g(T), g(T))). 

Given that we are considering an optimal control problem, the forces / 
must be chosen, if they exist, as the ones that extremize the cost func- 
tional: 

' G{qit),q{t),n{t))dt, (4) 

/o 

where G : TQ x U -^ R. 

The optimal equations of motion can now be derived using Pontryagin 
maximum principle. Generally, it is not possible to explicitly integrate these 
equations and, consequently, it is necessary to apply a numerical method. 
In this work, using discrete variational techniques, we will first discretize 
the Lagrange-d'Alembert principle and then the cost functional. We obtain 
a numerical method that preserves some geometric features of the original 
continuous system as we will see in the sequel. 

To discretize this problem we replace the tangent space TQ by the Carte- 
sian product Q X Q and the continuous curves by sequences qo,qi, ■ ■ -qN 
(we are using A^ steps, with time step h fixed, in such a way t^ = kh and 
Nh = T). The discrete Lagrangian L^ : Q x Q — )• R is constructed as an 
approximation of the action integral in a single time step (see |31] ) , that is 

"(fc+i)/i 



Ld{qk,qk+i)^ L{q{t),q{t)) dt. 

Jkh 



DISCRETE VARIATIONAL OPTIMAL CONTROL 5 

We choose the fohowing discretization for the external forces: fj^ : Q x Q x 
U -^ T*Q, where U C E,™, m < n, such that 



fk((ik,Qk+i,u+) e T*^+,Q- 

Observe that, as mentioned above, we have introduced the discrete con- 
trols as two different sequences {u^} and {u^}- In the notation followed 
through this paper, the time interval between [k, k + 1] is denoted as the 
fe-th interval, while the controls in k'^ and {k + 1)~ are denoted by n^ and 
^fc+i respectively. This choice allows us to model piecewise continuous con- 
trols, admitting discrete jumps at the time steps t^ = hk. Our notation is 
completely depicted in the following figure: 




hk 



h{k+l) 



h{k+2) 



h{k+3) 



Moreover, we have that 



fk ilk, %+i, % ) Sqk + fkilk, qk+1, Mfe ) Sqk+i 

f{k+l)h 

f{q{t),q{t),<t))5q{t)dt 



kh 

where {ff:{qk,qk+i,Uk)Jk{Qk,qk+i,ul)) € T*^Q x T*^^^Q (see [31j). 

Therefore, we derive a discrete version of the Lagrange-D'Alembert 
principle given in ([3]): 

N-l N-1 

^"^ Ld{qk,qk+i)+'^ [fk{qk,qk+i,u]:)6qk + f^{qk,qk+i,ul)6qk+i) = 0, 

fc=0 fc=o 

for all variations {5qk}k=o,...N with 5qk G Tq^^Q such that Sq^ = 5qN = 0. 
From this principle is easy to derive the system of difference equations: 

D2Ldiqk-i,qk) + DiLd{qk,qk+i) 

+fk-i{qk-i,qk,ul_^) + fk{qk,qk+i,u^) = o, (5) 
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where k = 1, . . . , N — 1. Equations ([5| are called the forced discrete 
Euler-Lagrange equations (see [35]). 

We can also approximate the cost functional Q in a single time step h 

by 

r{k+l)h 

Cd{qk,u^,qk+i,ut) ^ / C{q{t),q{t),u{t)) dt, 

Jkh 

yielding the discrete cost functional: 

N-l 

^ Cd{qk,u^,qk+l,u^) ■ 

k=0 

Observe that Cf^iQxC/xQxC/— >R. 

3.1. Fully-actuated Systems. In this section we assume the following 
condition 

Definition 3.1. (Fully actuated discrete system) We say that the dis- 
crete mechanical control system is fully actuated if the mappings 

^fc1fe,a.+i) ■ ^ ^ ^9*^^' fk\ig„,,^,)i^) = fki<lk,qk+l,u), 

^^^lte,..+0 ■ ^ ^ ^'?*.+i^' fk\i,„,,^,)(^) = f^{qk,qk+uu), 

are both diffeomorphisms. 

Define the momenta (see [3HI35]) 

Pk = -DiLd{qk,qk+i) - fk{qk,qk+i,u]:), (6) 

Pk+i = D2Ld{qk,qk+i) + fkiQk,qk+i,u^)- (7) 

Since both /;, L -, are diffeomorphisms we can express Uf, in terms of 

{qk,Pkiqk+i,Pk+i) using ^ and ([T]). Next, we define a new Lagrangian 
Cd : T*Q X T*Q ^ R by 

^d{qk,Pk,qk+l,Pk+l) = 
= Cd{qk , if^l^^^^g^^^^y^i-DiLd - Pk) , qk+1 , (8) 

The system is fully-actuated, consequently the Lagrangian Cd is well de- 
fined on the entire discrete space T*Q x T*Q. 

Now the discrete phase space is the Cartesian product T*Q xT*Q oi two 
copies of the cotangent bundle. The definition Q, ([T]) gives us a matching 
of momenta (see pT]) which automatically implies 

D2Ld{qk-i,qk) + fk^i{qk~i,qk,u'^^i) = -DiLdiqk,qk+i)- fk{qk,qk+i,u'^), 

k = 1, . . . ,N — 1, which are the forced discrete Euler-Lagrange equations 
([5]). In other words, the matching condition enforces that the momentum at 
time k should be the same when evaluated from the lower interval [fc — 1, A;] 
or the upper interval [k,k + 1]. Consequently, along a solution curve there 
is a unique momentum at each time t^, which can be called pk- 

The discrete Euler-Lagrange equations of motion for the Lagrangian Cd '■ 
T*Q X T*Q -^ R are 
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D3Cd{qk-i,Pk-i,qk,Pk) + DiCdiqk,Pk,qk+i,Pk+i) = 0, (9) 

D4Cd{qk-i,Pk-i,qk,Pk) + D2Cd{qk,Pk,qk+i,Pk+i) = 0. (10) 

In summary, we have obtained the discrete equations of motion for a 
fully-actuated mechanical optimal control problem as the discrete Euler- 
Lagrange equations for a Lagrangian defined on the product of two copies 
of the cotangent bundle. Therefore, all the preservation properties of the 
discrete equations Q and ( (Tol ) are now a direct consequence of the theory 
of variational integrators [3T] . 



3.2. Example: optimal control problem for a mechanical Lagrangian 
with configuration space E,". Consider the case Q = M" and assume that 
M is an n X n constant and symmetric matrix. The mechanical Lagrangian 
L : ]R2n ^ ]^ ig defined by L{x, ±) = \x^Mx - V{x), where F : R" ^ E 
is the potential function and x G M. The system is fully actuated and 
there exist no velocity constraints. The optimal control problem is typi- 
cally in terms of boundary conditions (a;(0),i;(0)) and {x{T),x{T)) for a 
given final time T. Note that in the continuous setting we can define the 
momentum by the continuous Legendre transformation FL : TQ -^ T*Q, 
{q,q) I— 5- {q,p)'- p = ^, i.e. p{t) = x^{t)M. In consequence, we can define 
boundary constraints also in the phase space: {x{{)) , p(0) = i;(0)-^M) and 
{x{T) , p{T) = x{Tf M). 

We set the Trapezoidal discretization for the Lagrangian (see |12j'l. that 
is, Ld{xk,Xk+i) = ^L{xk, ^thiz^) ^h^xk+i, ^i^+j^) where, as above, h 
is the fixed time step and xi,X2, ■ ■ ■ , xjy is a sequence of elements on W\ 
Our concrete discrete Lagrangian is 

Ld{xk,Xk+i) = ^(^^fc+i - Xk)'^M{xk+i - Xk) - - iV{xk) + V{xk+i)) . 

The control forces are fk{xk,Xk+i,u^) G ^4^" ^^'^ fki^k,Xk+i,ul) G 
T* R". For sake of clarity, we are going to fix the control forces in the 

following manner f^{xk,Xk+i,Ui^ ) = u^. Looking at equations (p| and 
is easy to obtain the associated momenta pk and Pk+i, namely 

1 h 

Pk = ■j^{xk+i-Xk)'^ M + -Vxixk)'^ -u~, 



1 h 



Let Cd = ^ Ylik=o [(""fc )^ + (""a^)^] ^^ ^ discrete approximation of the cost 
function. Consequently, the Lagrangian over T*]R" x T*MJ^ is 

^d{Xk,Pk,Xk+l,Pk+l) = 

f xk+i -Xk \ _ h 



T 




\ h 1 ^^-2^"(^'^)' 



P.+i-(^^±V^)^M + ^K.(x,+0 
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where Vx represents the derivative of V with respect to the variable x. Ap- 
plying equations ^ and (10) to £d we obtain the following equations: 



p.-( "^+^,7^-^ )"m-o, (11) 



P.-(^^±^f Af- jT4(x.f) (^M-^V^A^kf^ (12) 

where both set of equations are defined for A; = 1, ..., A^ — 1. It is quite 
clear that we could remove the pk dependence in equation ( [l2| ). However, 
we prefer to keep it in order to stress that the discrete variational Euler- 
Lagrange equations (|9| and ^ are defined in T*Q xT*Q {T*W x r*R" 



in the particular case we are considering in this example). 

Expressions pTj ) and (12) give 2{N — l)n equations for the 2{N + l)n 
unknowns {a;A;}fc=o > {Pk}k=o- Nevertheless, the boundary conditions 

xo = x(0), pQ=p{0), 

XN = x{T), pj^=p{T), 



contribute An extra equations that convert eqs. (11) and (12) in a nonlinear 



root finding problem of 2[N — l)n and the same amount of unknowns. 

3.3. Underactuated Systems. In this section, we examine the case of 
underactuated systems defined as follows: 

Definition 3.2. (Underactuated discrete system) We say that the dis- 
crete mechanical control system is underactuated if the mappings 

are both emheddings, that is, they are one-to-one immersions that are home- 
omorphisms of U to its image. 

Under this hypothesis we deduce that M-7 \ = fz\, \(U), 

•"^ {qk,qk+ij •'k l(gfe,(3fc+i)^ ^' 

J^t •, = fh\, ^(U) are submanifolds of T* Q and T* O, respec- 

tively. Therefore, /j^ L > are diffeomorphisms onto its image. Moreover, 

dimA^T, „ s=dimA^t „ s=dimL''. 
(gk,<ik+i) (qk,qk+i) 

The set of admissible forces is restricted to the space A47 ^ x A^ t s C 

^ {qk,qk+i) {qk,qk+l) 

T*^Q X T*^ Q. As a consequence, the set of admissible momenta defined 
in (|6]) and ([7]) satisfy 

{qk , -DiLdiqk, Qk+i) - Pk) S A^(g^,g,,^^) C T*^Q, 

{qk+i , -D2La{qk,qk+i)+Pk+i) e ^tqk,qk+i) ^ '^l+i^' 

Thus, the Lagrangian function defined in ([s]) is restricted to these points 
only. Thus, it is necessary to apply constrained variational calculus to derive 
the corresponding equations (see [2]). This is typically performed by means 
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of constraint functions ^^,^^ : T*Q x T*Q -^M,, l<a<n — dimU. 
Therefore the solutions of the optimal control problem are now viewed as 
the solutions of the discrete constrained problem determined by an extended 
Lagrangian £d and the constraints ^^. Since /^L x are embeddings, 

as established in definition (|3.2l), the number of constraints is determined 



by n minus the dimension of U. Note that the total number of constraints, 
$^, is therefore 2(n — dimC/). 

To solve this problem we introduce Lagrange multipliers (A^)",(A^)" and 
consider discrete variational calculus using the augmented Lagrangian 

^diqk,Pk,^k^1k+l^Pk+l, ^k ) =^d{Qk,Pkiqk+l,Pk+l) 

+ {Kr^a{<lk,Pk,qk+l,Pk+l) 

+ {^tT^t{qk,Pk,qk+i,Pk+i)- 

Observe that, in spite the constraints are functions of the Cartesian product 
of two copies of the cotangent bundle i.e. $^ : T*Q x T*Q — )• IR, neither 
$~ depends on Pk+i nor $+ on p/j. The discrete Euler-Lagrange equations 
gives us the solutions of the underactuated problem. 

Typically, the underactuated systems appear in an affine way that is 

fki<ik,qk+i,ul) = A~{qk,qk+i)+B~{qk,qk+i){u^) 
fk{qk,qk+i,ul) = A^{qk,qk+i)+B^{qk,qk+i){ul) 

wheie A~{qk,qk+i) e T*^Q, ^^ (%,gfc+i) e T*^+iQ- Moieovei B~ {qk, qk+i) G 
Lm{U,T*^Q) and B^{qk,qk+i) S Lin(C/, T^^ Q) are linear maps (we as- 
sume that [/ is a vector space and Lin(£'i , £'2) is the set of all linear 
maps between Ei and £'2)- In consequence BJ^ {qk, qk+i){u^) G '^qkQ ^^"^ 
B+{qk,qk+iKu+)eTl^^Q. 

Then the constraints are deduced using the compatibility conditions: 

rankS^ = rank {B~ ; -DiLd{qk,qk+i) - Pk - ^fc (9fc,9fc+i)) , 
rank5+ = rank (S+ ; -D2Ld{qk,qk+i) + Pk+i - A'^{qk,qk+i)) , 

which imply constraints in {qk,qk+i,Pk) and {qk,qk+i,Pk+i), respectively. 
The fact that f, \, . are both embeddings implies furthermore that 

rank BJ^ = rank B^ = dim U. 

4. Discrete optimal control on Lie groups 

An indispensable tool in the study of mechanical systems is reduction 
theory. Therefore, in this work we consider its discrete analogue. This is 
precisely the motivating idea of the work by Moser and Veselov [33], i-e. 
to give a discrete analogue of Euler-Poincare reduction. The approach is 
to reduces the standard second order Euler-Lagrange equations when the 
configuration space is a Lie group G to first order equations on the Lie 
algebra q. 

Following the developments in § [2] assume that the Lagrangian defined by 
-Lrf : G X G — )• R is invariant so that 



Ld{gk,gk+i) = Ld{ggk,g9k 



+ 1; 
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for any element g & G and {gk,gk+i) € G x G. According to this, we can 
define a reduced Lagrangian /^ : G — )• R by 

IdiWk) = Ld{e,g^^gk+i) 

where Wk = g^ gk+i and e is the identity of the Lie group G. 
The reduced action sum is given by 

{Wo,...,Wn-i) ^ Ek=oUWk). 
Taking variations of Sd and noting that 

SWk = -gk^{Sgk)g^^gk+i +g'k^Sgk+i = -VkWk + WkVk+i, 
where rjk = g^ Sgk, we arrive to the discrete Euler-Poincare equations: 
{rl^dld){Wk) - {ll^_^dld){Wk-i) = 0, k = l, ...,N- 1, 

where I : G x G ^^ G and r : G x G ^ G are respectively the left and the 
right translations of the group (see also |6j ) . 

If we denote by fik = {r* dld){Wk) then the discrete Euler-Poincare equa- 
tions are rewritten as 

^ik+i = -^dl^^f^k, (13) 

where Ad : G x g — )• g is the adjoint action of G on g. Typically this 
equations are known as the discrete Lie-Poisson equations (see [Sll28|[^). 
Consider a mechanical system determined by a Lagrangian Z : g — >• R, 
where g is the Lie algebra of a Lie group G, which also is a n-dimensional 
vector space. The continuous external forces are defined as follows / : g x 
?7 — )• g*. The motion of the mechanical system is described applying the 
following principle 

6[ l{^{t))dt+ [ {f{m,u{t)),v{t))dt = 0, (14) 

Jo Jo 

for all variations SS,{t) of the form S^{t) = r]{t) + [£,{t),'r]{t)], where r]{t) is an 
arbitrary curve on the Lie algebra with r/(0) = and ri{T) = (see [30]). In 
addition (•, •) is the natural pairing between g and g*. These equations give 
us the controlled Euler-Poincare equations: 

where ad^r] = [S,,r]]. 

The optimal control problem consists of minimizing a given cost func- 
tional: 

Cim,uit)))dt, (15) 

/o 

where G : q x U — > R. 

Now, we consider the associated discrete problem. First we replace the 

Lie algebra g by the Lie group G and the continuous curves by sequences 

Wq, Wi, . . . Wjsf (since the Lie algebra is the infinitesimal version of a Lie 

group, its proper discretization is consequently that Lie group [291 l3T]). 
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The discrete Lagrangian /^ : G — )• R is constructed as an approximation 
of the action integral, that is 

f{k+l)h 

idiWk) « / mt)) dt. 

Jkh 

Let define the discrete external forces in the following way: /^ : G x [/ — )• g*, 
where U C IR™" for m < n = dimg. In consequence 

fik+l)h 

{fkiWk,u,) , Vk) + {fjliWk,ut) , m+i) « / (/(e(t),n(t)),r?(t)) dt, 

Jkh 

where (/^(Wfc, u^), /^(W^, n^)) G g* x g* and % G g, for all k. In addition 
Vo = Vn = and (•, •) is the natural pairing between g and g*. 

For sake of simplicity we are sometimes going to omit the dependence on 
Gx C/ofboth/+ and/-. 

Taking all the previous into account, we derive a discrete version of 
the Lagrange-D'Alembert principle for Lie groups: 

Af-l N-1 

S Yl IdiWk) + J2 iifk^Vk) + {fhvk+i)) = 0, (16) 

fe=0 A:=0 

for all variations {5Wk}k=Q,...N-i verifying the relation 5Wk = —rjk Wk + 
Wkr]k+i with {r]k}k=i,...N-i an arbitrary sequence of elements of g which 
satisfies r]o,r]pf = (see [lElEI])- 

From this principle is easy to derive the system of difference equations: 

ll^_MdiWk-i)-rl/ld{Wk) 

+ fl,iWk^i,ut^,) + f,{Wk,u^) = 0, ^^^^ 

for k = 1, . . . ,N — 1, which are called the controlled discrete Euler- 
Poincare equations. 

The cost functional (15) is approximated by 

Cd{u^,Wk,u+)^ [ G(C(t),n(t))dt, (18) 

Jkh 

yielding the discrete cost functional: 

N-l 

J=Y.Cd{u^,Wk,ut). (19) 

fc=0 

Observe that now G^ : C/ x G x [/ — > R. 

4.1. Fully Actuated Systems. In the fully actuated case the mappings 
/^ 1^ :[/—)• g* defined by /^ | (n) = fj^ {W,u) are diffeomorphisms for all 
W £ G, therefore, we can construct the Lagrangian C^ '■ Q* x G x q* — ;■ K, 
by 

Cdii^k,Wk,i^k+i) 

CdiifkL r'K.dUWk)-i^k),Wk,utL r\-ii/id{Wk) + uk+i)), 

(20) 



'w, 



k 



12 F. JIMENEZ, M. KOBILAROV, AND D. MARTIN DE DIEGO 

where the variables Uk, fk+i G 0* are defined by 

^k = r*w^dld{Wk) - /fc"(Wfc,n-), 
Vk+i = r^^dhiWu) + /+(VFfc,n+), 



(21) 



The discrete phase space g* x G x g* is now a mixture of two copies of the 
Lie algebra g* and a Lie group G. This is also an example of a Lie groupoid 
(125]). 

The discrete optimal control problem defined in (16) and (18) has been 
reduced to a Lagrangian one, with Lagrangian function £rf : g* x G x g* — >■ R. 
In consequence, we are able to apply discrete variational calculus to obtain 
the discrete equations of motion in the phase space g* x G x g* . 

Let us show how to derive these equations from a variational point of view 
(see [25] for further details). Define first the discrete action sum 



N-l 

E 

fc=0 



Cd{vk-,Wk,Vk+i) 



Consider sequences of the type {{i'k,Wkii'k+i)}k=o,...,N-i with boundary 
conditions: uq, vn and the composition W = Wq Wi ■ ■ ■ Wn-2 Wn-i fixed. 
Therefore an arbitrary variation of this sequence has the form 

{z^A:(e) , ^fc ^(e) Wk hk+i{e) , Vk+i{<^)}k=o,...,N-i, 

with e G (—(5, 5) G IR (both e and 5 > are real parameters) and z^o(e) = ^Qi 
^A:(0) = Vk , i'n{^) = T^N-, hk{e) G G and /io(e) = h]\f{e) = e, for all e. 
Additionally /ife(O) = e for all k. 

The critical points of the discrete action sum subjected to the previous 
boundary conditions are characterized by 







d 
de 

d_ 
~de 



m-i 



e=0 



^£rf(ffc(e), /Jfc-^(e) Wfc/ifc+i(e), 1^^+1(6)) 



.fc=o 



e=0 



{Cd{vo,Wohi{e), ui{e)) + Cd{yi{e) , h^\e)Wih2{e) , u^ie)) 



+ . . . + Cd{h'N-2{<^) , h^\{e) Wn-2 hN-i{e) , J^Af-i(e)) 
+£rf(i^Ar_i(e) , /i^"'_i(e) Wn-1 , i^n)} ■ 

Taking derivatives we obtain 

7V-1 



= E K.dCd\ 



{uk-i,i^k) 



{Wk^i 



dCd\ 



fc=i 

Af-l 



(j^fe.l^fc+l) 



(Wk) 



+ z2 p2^rfi(Tyfc_i)(^*=-i'^*=) + DiCd\^^^^{i^k,'^k+i) 



6hk 
Si^k, 



where Cd 



(W) 



k=l 

: g*xg* 



R and Cd\f^^n '■ G ^ H are defined by Ca 



KW) 



(z., ,.') 



^d\( >\{W) = Cd{y, W, u'), where W e G and u, v' G g*. Since 5hk (which 



is defined as ^ I 



=0) and 5vk (which is defined as -^\^=q) 



k = 1,...,N-1 
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are arbitrary, we deduce the following discrete equations of motion: 
I* dCd\, AWk^i)-r*dCd\, ,{Wk) = 0, 

(22) 



for k = 1, . . . ^N — 1. Similarly to ^3.1 we obtain the control inputs Uj^ and 
ul using ^^. 

4.2. Underactuated Systems. The underactuated case can now be con- 
sidered by adding of constraints. Similarly to §3.3| underactuation restricts 
the control forces to lie in a subspace spanned by vectors {e''} of the basis 
{e**, e'^} of 0*, where {s,a} = 1, ...,n. Then 

/fc"(^fc.%) = H{Wk) + {hl{Wk,ul))se\ 

fki^kA) = al{Wk) + {bt{Wk,ul))se', 

where a^(Wfc),a+(Tyfc) G g* and (&^(Wfc,u^))„ (6+(Wfe,n+)), G R, for all 
s. Additionally, the embedding condition implies that rank 6^ = rank 5^ = 
dimC/. Then, taking the dual basis {6^,60-}, we induce the following con- 
straints: 

<^-{uk,Wk,iyk+i) = {r*^^dld{Wk) -Vk- al{Wk),ea) = 0, (23a) 

<^t(.'yk,Wk,iyk+i) = {i^k+i - ll/UWk) - al{Wk),e^) = 0. (23b) 



Observe in (23) that, even though the constraints are functions $^ : g* x 
G X g* — )• IR, neither <I>~ depends on Vk+i nor $+ on Vk- Once we have 
defined the constraints we can implement the Lagrangian multiplier rule in 
order to solve the underactuated problem. Namely, we define de extended 
Lagrangian as: 

^d{T^k, A^, Wk, Vk+i, Xl) =Cd{vk, Wk, i^k+i) 

+ {X^rK{'^k,Wk,Uk+i) (24) 

+ {Xtr<^t{^k,Wk,Uk+i). 

Defining the discrete action sum 

N-l 
^under ^ ^ £,(z.fc, A^ , VFfc, l^fc+i, A+), 
A:=0 

we obtain the underactuated discrete equations of motion 
C dCdlf JWk-i)-r* dCd\, dWk) 



+ C,.a(^^-i)^^^^lK-.^.)(^^-) + (^^i)'^^^^^l(^.-.^.)(^^-) 



D2 lld\^yj^_^-^{vk-uvk) ^ Dx lld\i^y^^^{yk,Vk+\) + [(A^_i)'' - (A^)''] e^ = 0, 

^~{Vk.Wk,Vk+\) = ^, 

$+(i/fc,Wfc,z^fc+i) = 0, 

(25) 
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where the subscripts (Wk-i), (Wk), (z//c_i,ffc), (i/^, i/^+i) denoted variables 
that are fixed. 

5. Numerical Methods for Systems on Lie Groups 



We now put the discrete optimal control equations (22) and (25) into a 
form suitable for algorithmic implementation. The numerical methods are 
constructed using the following guidelines: 

(1) good approximation of the dynamics and optimality, 

(2) avoid issues with local coordinates 

(3) guarantee for numerical robustness and convergence, 

(4) numerical efficiency. 

The discrete mechanics approach provides an accurate approximation of the 
dynamics (requirement [I| through momentum and symplectic form preser- 
vation and good energy behavior. In addition, we will satisfy requirement [2] 
for systems on Lie groups by lifting the optimization to the Lie algebra 
through a retraction map that will be defined in this section. The result- 
ing algorithms are numerically robust in the sense that there are no issues 
with coordinate singularities and the dynamics and optimality conditions 
remain close to their continuous counterparts even at big time steps. Yet, 
as with any other nonlinear optimization scheme it is difficult to formally 
claim that the algorithm will always converge (requirement p| . Nevertheless, 
in practice there are only isolated cases for underactuated systems that fail 
to converge. A remedy for such cases has been suggested in |18j . In general, 
the resulting algorithms require a small number of iterations, e.g. between 
10 and 20 to converge (requirement El) . 

The optimization variables W^ are regarded as small displacements on the 
Lie group. Thus, it is possible to express each term through a Lie algebra 
element that can be regarded as the averaged velocity of this displacement. 
This is accomplished using a retraction map r : g — t- G which is an 
analytic local diffeomorphism around the identity such that t(^)t(— ^) = e, 
where C S g. Two standard choices for r are employed in this work: the 
exponential map, and the Cayley map. 

Regarding ^ as a velocity we set the discrete Lagrangian Z^ : G — ;■ R to 

where ^k = t'^ {g^^ gk+i) / h = T''^{Wk)/h. The difference g^^ Qk+i G G, 
which is an element of a nonlinear space, can now be represented by the 
vector S,k in order to enable unconstrained optimization in the linear space 
for optimal control purposes. 

The variational principle will now be expressed in terms of the chosen 
map r. The resulting discrete mechanics will thus involve the derivatives of 
the map which we define next (see also [71 [T3| [TH] ) : 

Definition 5.1. Given a map r : g — t- G, its right trivialized tangent 

dr^ : — ;■ B and is inverse drJ : — > 0, are such that for g = r(^) G G 
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and r] £ g, the following holds 

d^T-\g)ij=dT^\r]T{-0). 
Using these definitions, variations 5(, and 6g are constrained by 
^^k = dr^^^li-rik + Adr(hCk)Vk+i)/h, 

where % = g'j^ Sg^, which is obtained by straightforward differentiation of 

6 = T'^{9k^ 9k+i)/h. 

The retraction map r choices are: 

a) The exponential map exp : g — )• G, defined by exp(,^) = 7(1), with 

7 : K, — )• G in the integral curve through the identity of the vector field 
associated with ^ G 3 (hence, with 7(0) = ^). The right trivialized derivative 
and its inverse are defined by 



00 ^ 
dexp^ y = X] ~r~^r^\ ^^^' ^' 



00 



(i + 1) 



dexp^iy = ^^ad^y, 
i=o ^' 

where Bj are the Bernoulli numbers (see [12]). Typically, these expressions 
are truncated in order to achieve a desired order of accuracy. 

b) The Cayley map cay : g — )■ G is defined by cay(^) = (e — |)~^(e + |) 
and is valid for a general class of quadratic groups (see [12]) that include the 
groups of interest in this paper (e.g. SO{'i), SE{2) and SE(?,)). Its right 
trivialized derivative and inverse are defined by 

dcay^y = [e - -)-^ y {e + -)-^ , 

dcay-^ = (e--)y(e + -). 

Next, the discrete forces and cost function are defined through a trape- 
zoidal approximation, i.e. 

and 

respectively. With the choice of a retraction map and the trapezoidal rule 
the equations of motion ( 13 ) become 

f^k - Ml^f^^^^^^Hk-i = -^ f{Ck,u^) + ^ f{^k-i,ut.i), 

f^k = {dT;;l^Td^i{Ck), 
Ok+i = gkT{h^k), 
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while the momenta defined in (|21|) take the form 



h 



fJ-k 



f{ik,u^ 



k h 
h 



^<i*r{hiik)t^k + ^f{(k,U 



(26) 
(27) 



t^k+l 

Finally, define the Lagrangian ^^^ : g* x g x g* — )• K, such that 

Note that the Lagrangian is well-defined only on g* xilxg*, where 11 C g is an 
open neighborhood around the identity for which r is a diffeomorphism. To 
make the notation as simple as possible we retain the Lagrangian definition 
to the full space g* x g x g*. 



The optimality conditions corresponding to (|22|) become 



-h^k 



didlf ■Mk^i)-{(^rr;)*d£d\f ^(Cfc) 

^2^1^, .^(^^fc-l,^^fc) + Died\(f:,Ai^k,i^k+l) 



l&-i)^ 



!&)' 



0, (28) 
0, (29) 



for k = 0,...,N - 1. Here, 



1(5) 



,z^, 1/ 



^U^>){0 = (■d{v,i,i''). Equations 



(28) and (29) can be also obtained from (22) employing Lemma 8.2 and 
Lemma 8.3 in Appendix A. 

In the underactuated case we define 



h{v,i,v\\ ,X^) =Cd{v,T{hi),v') 



+ {\-rK\^^,^,^{r{hi)) + (A+)'^$+|(^_^,)(r(/iO), 



(30) 



and from ( 25 ) obtain the equations 

Mk-i) 



(^-=kJ*d> 



K^k- 



.^fc.^J-l)^ 



D2Cd\^i^f:^ ^-^{uk-i.Vk) + DiCd\ 



^r(Mk 

$+(z^fc,r(/i6),z^fc+i) = 0, 
where we employed the notation A 



r{hiik) 



{uk,i^k+i) + A 



(^k,t^k+i,^h ) 



fc-i 



A7 



(6) = 0, 
= 0, 



(31) 



(A 



±\a. 



Boundary Conditions.: Establishing the exact relationship between the 
discrete and continuous momenta, ^k and /x(t) = d^l{^{t)), respectively, is 
particularly important for properly enforcing boundary conditions that are 
given in terms of continuous quantities. The following equations relate the 
momenta at the initial and final times t = and t = T and are used to 
transform between the continuous and discrete representations: 

/zo- 5^/(^(0)) = J/(e(0),^o 



a^/(C(r))-Ad;(,^^_^)^^_i 



2 



which also corresponds to the relations vq = 5^/(^(0)) and vn = d(l{^{T)). 
These equations can also be regarded as structure-preserving velocity bound- 
ary conditions, i.e., for given fixed velocities .^(0) and ^(T). 
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The exact form of the previous equations depends on the choice of r. 
This choice will also influence the computational efficiency of the optimiza- 
tion framework when the above equalities are enforced as constraints. The 
numerical procedure to compute the trajectory is summarized as follows: 

Algorithm 5.2. Optimal control 

Data: group G; mechanical Lagrangian I; control functions a, b; cost 
function C ; final time T; number of segments N. 

(1) Input: boundary conditions {g{0),(,{0)) and (g{T),£^(T)). 

(2) Set m,omenta vq = 9^/(^(0)) and ujy = d^l{S,{T)) 

(3) Solve for {^0, ...j^j^^ijUi, ...,iy]\[^i, X^ , ..., Xj^_^) the relations: 
equations ( |31[ ) for all k = 1, ...,N — 1, 
r-i {T{hCN^irK..T{hCo)-' 9{0)-'9{T)) = 

(4) Output: optimal sequence of velocities £,0, ...,^N-i- 

(5) Reconstruct path go,..., gN by gk+i = gkT{h^k) for k = 0, ...,N -1. 

The solution is computed using root-finding procedure such as Newton's 
method. If the initial guess does not satisfy the dynamics we recommend 
to use a Levenberg-Marquardt algorithm which has slower but more robust 
convergence. 

5.1. Example: optimal control effort. Consider a Lagrangian consisting 
of the kinetic energy only 

full unconstrained actuation, no potential or external forces and no velocity 
constraint. The map I : g — ?■ g* is called the inertia tensor and is assumed 
full rank. 

In the fully actuated case we have f{^k,Uj^ ) = Uj^ . We consider a mini- 
mum effort control problem, i.e. 

C{tu) = l\\uf. 

The optimal control problem for fixed initial and final states (^(O) , ^(0)) 
and {g{T) , ^(T)) can now be summarized as: 

Compute: ^q-.n-i , u^.j^, 

minimizing: \ J2k=o (II % f + II ^fc f ) ' 

subject to: 

/.o-I(C(0)) = |no, 

I(^(T))-Ad;(,^^_^)^^_i = §n+, 

//fc = (dr,7j*I(a), 

gk+i= 9kr{hCk), k = 0,...N -1, 

T'Hg],'giT)) = 0. 
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The optimality conditions for this problem are derived as follows. The 
Lagrangian becomes 



N-l 



k=0 



where the momentum has been computed according to 

^k = \ ((dr,/j*Ife) + (drr,^^^_J*I(a-i)) , (32) 

Thus the optimality conditions become 

(drr/)*d^J. ^(^fc)-(dW. )*Md\, (^;-,_i) = 0, 

k = l,...,iV- 1, 

T-^ [rihin-xY^ .■.T{hi^-' 9-^^g{T)) = 0. 

It is important to note that these last two equations define A'^ • n equations 
in the Nh unknowns ^o:Af-i- A solution can be found using nonlinear root 
finding. Once ^q-n have been computed, is possible to obtain the final con- 
figuration qn by reconstructing the curve by these velocities. Beside, the 
boundary condition g{T) is enforced through the relation T~^{g~^ g{T)) = 
without the need to optimize over any of the configurations g^ ■ 

5.2. Extension: the configuration-dependent case. The developed frame- 
work can be extended to a configuration-dependent Lagrangian L : G x g — )■ 
R, for instance defined in terms of a kinetic energy i^ : g — t- M and potential 
energy F : G — )■ E, according to 

L{g,i) = K{i)-V{g), 

where g G G and ^ G 0. The controlled Euler-Poincare equations are in this 
case 

/i - ad|^ = -/* dg V{g) + /, 

g = g^, 



where the external forces are defined as/:Gxgx[/— s-g*. Our discretiza- 



tion choice Ld : G x G ^ M. will be (recall that ^k = t ^(s'fc gk+i)/h) 



h h 

Ld{gk,gk+i) = -L{gk,Ck) + ^L{gk+i,S,k) 

while the G-dependent discrete forces now become 

fk{gk,Ck,u^) = ^f{gk,Ck,u^), fk{gk+i,Ck,ul) = ^f{gk+i,Ck,ul). 
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This leads to the discrete equations 

+ 2 /(5fc, ik. % ) + - /(5fc, Cfc-i, ^fc_i), 

gk+i = 9kT{h^k)- 
The momenta become 

\n.d,yi^9k)-\ 

In consequence, we can define a discrete Lagrangian 

-Cd : g* X G X g X g* ^ R, 
depending on the variables {I'kjdk, Ck, t'fc+i) which discrete equations of mo- 



T^k = f^k + 7^ l*g^ dg V{gk) - - f{gk, ^k,Ui^ , 
vk+i = Adl(^^^^-^fik-'jl*g^^,dgV{gk+i) + '-figk+i,Ck,Uk 



tion will be a mixture between (22) and (28), (29), namely 



+ f(dr'.V )* d&d\i ^(Cfc-l) - (dr."/)* ^-CdL ^{^k)) = 0. 

6. Applications 

6.1. Under'water Vehicle. We illustrate the developed algorithm with an 
application to a simulated unmanned underwater vehicle. Figure (IT]) shows 
the model equipped with five thrusters which produce forces and torques 
in all directions but the body- fixed "y"-axis. Since the input directions 
span only a five-dimensional subspace the problem is solved through the 
underactuated framework. 

The vehicle configuration space is G = SE{2>). We make the identification 
SE{?,) ~ S0{?,) X E^ using elements R £ S0{3) and x£R^ through 

_ ( R x\ -1 _ ( R^ -R^x 

^ " V 03X3 1 y ' ^ "V 03X3 1 

where g E SE{2>). Elements of the Lie algebra ^ E se(3) are identified 
with body- fixed angular and linear velocities denoted a; E Hl^ and u E R^, 
respectively, through 

(J V 



^ V 03X3 

where the map " : R^ — )• so (3) is defined by 



— wa ijj2 
uj = \ uj^ —uji I . (33) 

-UJ2 OJi 
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propeller 



Figure 1 . An underwater vehicle model (a) and a various computed 
optimal trajectories between chosen states (b). Only a few frames along 
the path are shown for clarity. 



Angular velocity 



Linear velocity 



Control inputs 



I op*'''^-^rrnrtttm*y 



2 4 

sec. 




Figure 2 . Details of the computed optimal path for the reconfigura- 
tion maneuver given in Figure M. 



The algorithm is thus implemented in terms of vectors in M^ rather than 
matrices in 5e(3). 

The map r = cay : sc(3) — )■ SE{3) is chosen, instead of the exponen- 
tial, since it results in more computationally efficient implementation. It is 
defined by 

where cay : so (3) — ;■ 50(3) is given [^ by 



cay(a;)=l3 + ^^ (^ + - 



(34) 



where I„ is the n x n identity matrix and dcay : ]R^ — )• E,^ is defined by 

2 



dcay^ 



4+ w 



.(2l3+^). 



(35) 



note that cay denotes a map to either 5*0(3) or SE(3) which should be clear from its 
argument. 
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The matrix representation of the right-triviahzed tangent inverse dr 



becomes 



[dcay 



{uj,v)i 






03 



lO; 



(u],v) 



(36) 



2^) ^ ^.i 2"- 

The vehicle inertia tensor I is computed assuming cyhndrical mass dis- 
tribution with mass m = 3kg. The control basis vectors are {es}^s=i ~ 



IS 



{61,62,63,64,65}, while the non-actuated direction is e^ = eg, where 6j 
the i-th standard basis vector of M^. The control functions take the form 

h{W,u)i = d{u5 -Ui), 

b{W,u)2 = c{iui+U2)/2-U3), 

vr 
b{W, u)3 = (csin -){u2 - til), 

b{W, n)4 = U1+U2 + Us, 

b{W,u)5 = U4, + U5, 

a{W) = Ht-^{W), 

here i7 is a negative definite viscous drag matrix and the constants c, d are 
the lengths of the thrusting torque moment arms (see Figure [I| . 

We are interested in computing a minimum control effort trajectory be- 
tween two given boundary states, i.e. conditions on both the configurations 
and velocities. Such a cost function is defined in ^5.1 The optimal control 



problem is solved using equations (31). The computation is performed us- 



ing Algorithm 5.2 Figure [2] shows the computed velocities and controls for 
the "reconfiguration" trajectory shown in Figure [T] The algorithms requires 
between 10-20 iterations depending on the boundary conditions and when 
applied to A^ = 32 segments. 




Forces 




a) 



Figure 3. An optimal trajectory of an underactuated rigid body 
on 50(3) (a). The body is controlled using two force inputs around 
the body-fixed x and y axes. An Li-control cost function results in a 
discontinuous optimal trajectory (b) which our algorithm can handle. 



6.2. Discontinuous Control. One of the advantages of employing the dis- 
crete variational framework is the treatment of discontinuous control inputs 
as illustrated in ^ The nature of the control curve depends on the cost 
function. In the standard squared control effort case (i.e. L2 control curve 
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norm employed in ^6.1[ ) the resulting control is smooth. Another cost func- 
tion of interest is L \\u{t)\\dt (i.e. the Li control curve norm) which is 
typically imposed along with the constraints Umin ^ u{t) < Umax- This 
case results in a discontinuous optimal control curve. Our formulation can 
handle such problems easily since the terms u^ and u^ are regarded as the 
forces before and after time tk, respectively. A computed scenario of a rigid 
body actuated with two control torques around its principles axes of inertia 
(Fig. [3]) illustrates the discontinuous case. 

7. Extensions 

The methods developed in the previous sections are easily adapted to 
other cases which are of great interest in real applications. In particular, this 
section will be devoted to the discussion of two important extensions: the 
case of optimal control problems for Lagrangians of the type / : TM x g — )• K, 
(that is, reduction by symmetries on a trivial principal fiber bundle) and 
the case of nonholonomic systems. Here, M denotes a smooth manifold. 
Observe that the phase space TM x q unifies the previously studied cases 
of a tangent bundle and a Lie algebra. 

The notion of principal fiber bundle is present in many locomotion and 
robotic systems [5l[8l[27]. When the configuration manifold is Q = M x G, 
there exists a canonical splitting between variables describing the position 
and variables describing the orientation of the mechanical system. Then, 
we distinguish the pose coordinates g £ G (the elements in the Lie algebra 
will be denoted by ^ G g), and the variables describing the internal shape 
of the system, that is x G M (in consequence {x,x) G TM). Observe that 
the Lagrangians of the type / : TM x g — t- IR, mainly appears as reduction of 
Lagrangians of the type L : T{M x G) — ?■ R, which are invariant under the 
action of the Lie group G. Under the identification T{M x G)/G = TM x g 
we obtain the reduced Lagrangian I. We first develop the discrete optimal 
control problem for systems in an unconstrained principle bundle setting 



in ^7.1 Nonholonomic constraints are then added to treat the more general 



case of locomotion systems in { 7.2 



7.1. Discrete Optimal Control on Principle Bundles. The discrete 
case is modeled by a Lagrangian l^ : M x M x G ^ M. which is an approxi- 
mation of the action integral in one time step 

ld{xk,Xk+i,Wk) ^ / l{x{t),x{t),^{t)) dt, 

Jhk 

where {xk, Xk+i) £ M x M and Wk S G. Again, we make an election for the 
discrete control forces /^ : M x M x G x [/ -> r*M x g*, where U CBJ^: 

fkixk,Xk+i,Wk,u^) = \^fkixk,Xk+i,Wk,u^),f^ixk,Xk+i,Wk,u~)j , 

f^{xk,Xk+i,Wk,u^) = \^f^{xk,Xk+i,Wk,u'^),fkixk,Xk+i,Wk,u^)j , 

here 4" G T*^M x g* and /+ £ T*,^^M x g* (more concretely f^ E T^M, 
/^er4^M,/-Gg*,/+eg*). 
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Similarly to the developments in § |3] and § 4.1 we can formulate the 
discrete Lagrange-D'Alembert principle: 



AT-l N~l 



k=0 



fc=0 k=0 

N-1 



k=0 

which can be rewritten as 

Af-l 7V-1 7V-1 



^ X] ^d{Xk,Xk+l,Wk) + ^ 4 6Xk + ^ fk^Xk+i 

k=0 k=0 

N-1 N~l 



k=0 k=0 k=0 

N-1 N~l 



k=0 k=0 

for all variations {(5xfc}^=Q with 5xk G T^^M and 5xq = Sxn = 0; also 
{SWk}k=o witli ^^k G Tg^G, such that 5Wk = -r]kWk + Wkm+i, being 
{rik}k=o ^ sequence of independent elements of g such that rjo = rjj^ = 0. 

Applying variations in the last expression and rearranging the sum, we 
finally obtain the complete set of forced discrete Euler-Lagrange equa- 
tions: 

Dild{xk,Xk+i,Wk) +D2ld{xk-i,Xk,Wk-i) + fk + U~i = 0' (37) 

l*Wk-i^?'^d{xk-i,Xk,Wk-i) - r*^^D^ld{xk,Xk+i,Wk) + /^ + fk-i = 0'(3^) 

with A; = 1,...,A^ — 1. Since we are dealing with an optimal control problem, 
we introduce a discrete cost function C^^ : M x G x M x [/ x J7 — t- H. As in 
previous cases, our objective is to extremize the following sum 

Af-l 



y^ Cd{xk,Wk,Xk+i,u^,,ul), 



k=0 



subjected to equations (37) and (38). Let us initially restrict our attention 



to the case of fully actuated systems. 

Definition 7.1. (Fully actuated discrete system) We say that the dis- 
crete mechanical control system is fully actuated if the mappings 

4"l(xo,x„iyi) ■ U ^ T*,M X r, /fc"|(,„,,„H',)M = 4-(xo,xi, VFi,n), 
are both diffeomorphisms. 
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According to equations (37) and (38), we can introduce the momenta by 



means of the following discrete Legendre transforms: 
Pk = -Dild{xk,Xk+i,Wk) -/^, 
Pk+i = D2ldixk,Xk+i,Wk) + fk, 



P-k = r'{^^^D-ild{xk,Xk+i,Wk) - fk , 
A^fc+i = Iwk^^ldixk, Xk+i,Wk) + f^. 

In the fully actuated case, is possible to find the value of all control forces 

in terms oi Xk,Xk+i,Wk,Pk,Vk+i, P-k, P'k+ii that is: 

% = u]^{xk,Xk+i,Wk,Pk,fJ'k), (39) 



u^ixk, Xk+i, Wk,Pk+l,^J'k+l) 



(40) 



Replacing (39) and (40) into Cd, we finally obtain the discrete Lagrangian 



that completely describes our system: 

Cd : T*M XQ* xGxQ* X T*M 
The associated discrete cost functional is 



R. 



N-l 



Jd = /_^ ^dixk^PkjfJ-kjWk, tJ-k+l,Xk+l,Pk+l] 



(41) 



fc=0 



As usual, we take now variations in (41) in order to obtain the discrete 



Euler-Lagrange equations for our optimal control problem (with some abuse 
of notation we denote Qk = {xk,Pk, IJ-kjWk, p-k+ijXk+iTPk+i) the whole set 
of coordinates in the new phase space): 



De£diQk-i) + 


DiCd{Qk) = , 


DjCdiQk-i) + 


D2Cd{Qk) = , 


D^CdiQk-i) + 


D^Cd{Qk) = , 


^Wk-i^4:^diQk-l) - 


r*w^D,Cd{Qk) = 



0, 



together with the forced discrete Euler-Lagrange equations (37) and (38). 



Typically, actuation is achieved by controlling only a subset of the shape 
variables. In our setting this is can be regarded as underactuation - 



the mappings in definition 7.1 become embeddings. If this is the case, it is 



necessary to introduce constraints and apply constrained variational calculus 
as in 



3.2 and S 4.1 



7.2. Discrete Optimal Control of Nonholonomic Systems. This sub- 
section is devoted to add nonholonomic constraints to the picture. Holo- 
nomic constraints might be considered as a pacticular case of the nonholo- 
nomic ones (see [23j for further details). With this extension it would be 
possible consider examples of optimal control of robotic vehicles. In the fol- 
lowing we will expose the theoretical framework, leaving for future research 
the application to concrete examples. 

A controlled discrete nonholonomic system on M x M x G is given by the 
following quadruple (see [HI |20] ) : 

i) A regular discrete Lagrangian Id : M x M x G ^ R. 
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ii) A discrete constraint embedded submanifold Aic of M x M x 

G. 
in) A constraint distribution, Dci which is a vector subbundle of the 
vector bundle Tj,^,/xb • '^^^ x — ^ -^) such that dirriA^c = dimPc. 
TypicaUy, there is a relation between the constraint distribution and 
the discrete constraint, since from Mc we induce for every x G M, 
the subspace 'Dc{x) of TxM x q given by 

Vc{x) = T(^,,x,e)Mc n {T,M X g) , 

where we are identifying TxM x 3 = 0^ x TxM x T^G, with e being 
the identity element of the Lie group G. 
iv) The discrete control forces /^ : Mc xU ^ T*M xq* where U C R™ 
(again, forces /^ split into /^, and /^ as in the previous section). 
We have the following discrete version of the Lagrange-D'Alembert 
principle for controlled nonholonomic systems: 
N-l N-1 

s'^id{xk,xk+i,Wk) + y^ifkA^xk^m)) 

fe=0 A;=0 

N~l 

+ Yl ^i'k^ {Sxk+wnk+i)) = 0, 

k=0 

for all variations {5xk}k=Q^ with 5xq = 6xj^ = 0; and {5Wk}k=o^ such that 
5Wk = -r]kWk + Wk'nk+i, being {r]k}k=o, verifying {6xk,r]k) £ V^Xk) C 
Txf.M X Q such that rjo = tjn = 0. Moreover, {xk,Xk+i,Wk) G Mc, k = 
0,...,N -I (see [H]). 

Take a basis of sections {(X'*,f/")} of the vector bundle r©^ : Vc — > M, 
where X"" G X{M) and fj"' : M ^ q for a = 1, ..., rank(Pc)- Hence, the equa- 
tions of motion derived from the discrete Lagrange-D'Alembert principle for 
controlled nonholonomic systems are: 

- {DMxk,Xk+i, Wk) + D2ld{xk~uXk,Wk-i) + J- + /+_! , X\xk)) 

(42) 
+ {l*Wk_^D'ild{xk-i,Xk,Wk^i) ~ r*^^Dzld{xk,Xk+i,Wk) + fk + fk-i , v'^ixk)), 

= *"(xfe,Xfe+i,W^fe), (43) 

where ^°'{xk,Xk+i,Wk) = are the constraints which locally determine 
Md. 



In a more geometric way, we can write equations ( 42 ) and ( 43 ) as follows 
= {iVc)*\^Dildixk,Xk+i,Wk) + D2ld{xk-i,Xk,Wk-i) + f^ + f,l_^^, 

l^^_^D3ld{xk~i,Xk,Wk-i) - r'l^^^D3ld{xk,Xk+i,Wk) + fk + fk-i), 



where(xfc, Xk+i, Wk) G Mc and iv^ '■ ^c ^^ TMxq is the canonical inclusion. 
Given a discrete cost function Cd '■ U x Mc^ U — > IR, and the optimal 
control problem is to minimize the action sum 

N-l 

X] Cd{u~k ,Xk,Wk,Xk+l,ul) 

k=0 
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subject to equations (42) and (43) and to some given boundary conditions. 
We next distinguish between the fully and under-actuated case using the 
following definition: 

Definition 7.2. (Fully actuated nonholonomic discrete system) We 

say that the discrete nonholonomic mechanical control system is fully actu- 
ated if the mappings 

are both diffeomorphisms for all {xq, xi, Wi) G Aic- 



Regarding equation (42) and its geometric redefinition just below, let 
introduce the following momenta: 

tta: = (ivj* \^-Dild{xk,Xk+i,Wk) - fk,rWkD3ldixk,Xk+i,Wk) - f^j , 

TTfc+i = {ivj* \^D2ld{xk,Xk+l,Wk)+fkJwk^3id{xk,Xk+l,Wk) + fk 

where both vr^ and TTk+i belong to P*. In the fully actuated case, the value of 
all control forces can be completely determined in terms oixk, Xk+i, Wk, vTfc, Tr^+i, 
where the coordinates {xk,Xk+i,Wk) always belong to Aic- Therefore we 
can re-express the cost function in terms of these variables and, in conse- 
quence, derive the discrete Lagrangian 

where pri :A^rfCMxMxG— t-M are the projections onto the first and 
second arguments and Tpj : P* — )■ M the vector bundle projection. 

Observe that we can consider this case as a constrained discrete variational 
problem taking an extension 

Zd:V*xGxV*^R 

of Cfi subjected to the constraints ^"(x^, x^+i, Wk) = 0. 

Therefore, denoting Qk = {xk,TTk,Wk,Xk+i,TTk+i) as the whole set of 
coordinates of the new phase space P* x G x "D* , we deduce that the equations 
of motion are 

D^CdiQk-i) + D.ZdiQk) = \t'D2^''{xk-i,Xk,Wk-i) 

DMQk^i) + D2Zd{Qk) = 0, 

l*w,_,DsZi{Qk^i) - r*yy^DMQk) = A^i/^^_^Z)3^"(xfc„i,Xfc,VF,._i) 

-Xyw,D3^''{xk,Xk+i,Wk), 

^°(xfc,Xfc+i,Tyfc) = 0, 

where A„ are the Lagrange multipliers of the new constrained problem. The 
underactuated case can be handled by adding new constraints and applying 
discrete constrained variational calculus similarly to Q 
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A natural framework that simplifies the previous construction is based 
on discrete mechanics on Lie groupoids [25]. The Lie groupoid structure 
generalizes the case of Q x Q, the Lie group G and also many intermediate 
situations. In particular, many of the examples studied in this paper can be 
modeled using Lie groupoid techniques adapted to our formalism (see [E]). 



8. Conclusions 

This paper develops numerical methods for optimal control of Lagrangian 
mechanical systems defined on tangent bundles, Lie groups, trivial principal 
bundles, and nonholonomic systems. The proposed approach preserves the 
geometry and variational structure of mechanics through the discretization 
of the variational principles on manifolds. The key point is to solve the opti- 
mal control through discrete mechanics, i.e. by formulating the optimization 
as the solution of an action principle of a higher-dimensional system in a new 
Lagrangian phase space, i.e. T*Q xT*Q in the general case and q* x G x q* 
in the Lie group case. The optimal control algorithm is then derived as a 
variational integrator subject to boundary conditions. We thus expect that 
both the dynamics and optimal control solutions will have accurate and sta- 
ble numerical behavior (due to symplectic-momentum preservation) even at 
large time-steps (which allows for improved run-time efficiency). 

Simulations of an underactuated underwater vehicle illustrate an applica- 
tion of the method. Yet, further numerical studies and comparisons would 
be necessary to exactly quantify the advantages and the limitations of the 
proposed algorithm. An important future direction is thus to study the con- 
vergence properties of the optimal control system. Convergence for general 
nonlinear systems is a complex issue. In this respect, it is interesting to note 
that the discrete mechanics and optimal control on Lie groups such as the 
example in using the Cayley map results in polynomial form without further 
approximation or Taylor series truncation. A useful future direction is then 
to study the regions of attraction of the numerical continuation using tools 
from algebraic geometry. 

More generally, the theoretical framework introduced in ^ can serve as 
a basis for deriving algorithms for control systems such as multi-body loco- 
motion systems or robotic vehicles with nonholonomic constraints. Further- 
more, the developed classes of systems can be unified through the recently 
developed groupoid framework [lH [37]. Each of the considered product 
spaces (e.g. QxQ) can be regarded as a single groupoid space with equations 
of motion resulting from a single generalized discrete variational principle. 
This will enable the automatic solution of optimal control problems for var- 
ious complex systems and a convenient unified framework for implementing 
practical optimization schemes such as [H [181 HI [35] . More importantly, 
this viewpoint can be used to apply standard discrete Lagrangian regularity 
conditions (e.g. [31j) to optimal control problems evolving on the groupoid 
space. This would provide a deeper insight into the solvability of the result- 
ing optimization schemes. 
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Appendix A: Lemmae 

Lemma 8.1. (see [30\) Let g £ G, X £ q and 6f denote the variation of 
a function f with respect to its parameters. Assuming A is constant, the 
following identity holds 

5{AdgX) = -Adg[X,g-%], 

where [■ , ■] : g x g — )• R denotes the Lie bracket operating or equivalently 
[C,v] = ad^V, for given rj, ^ e q. 

Lemma 8.2. (see [7\) The following identity holds 

drg rj = Ad^(^) dr_^ r/, 
for any ^,rj £ q. 

Lemma 8.3. (see \7]) The following identity holds 

dr~^ ri = drZ^ (Ad^(_^) ??) , 
for any C,V ^ Q- 
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